VOL  7^. 


NORTHROP 


Electronics  Division 


□ This  submittal  applies  to  AN/BRN-7  (Submarine  n)  only. 

□ This  submittal  applies  to  AN/SRN-( ) (Hydrofoil  «)  only. 

| XI  This  submittal  applies  to  both  AN/BRN-7  and  AN/SRN-( ). 

CONTRACT  NO:  N00039-73-C-0209 

PROGRAM  NAME:  AN/BRN'7 


CDRL  No:  A01D , A01E,  A01F 

Title  of  CDRL : rnmpnter  Program  nesien  Snpir i f icatlon 
Computer  Subprogram  Design  Document 
Data  Base  Design  Document 


Title  of  Doc:  AN/BRN-7  Computer  Program  Specification 
NORT  73-48 
Volume  2 thru  13 


Date: 

Initial  Submittal: 
Resubmittal: 


1/16/74 


Release 

Authentication 

Prepared  by:  System  Integration  Unit 

Orgn.  Name 


Supervisor  of  Preparing 
Orgn. 

FORM  6912  (2-  721 


A4734 X574  _ 

Orgn.  No.  and  Ext. 


Approved  by: 


D D C 


NORTHROP 


Electronics  Division 


Copy  No._ 


NORT  73-48 

AN/BRN-7  COMPUTER 
PROGRAM  SPECIFICATION 

Volume  VI 

KALMAN  FILTER  SUBPROGRAM  DESIGN 
October  12,  1973 


JSffSSIM  if 


Fw'rr's 50 


Approved  b\ 


G.  W.  Hauser 
Director,  Engineering 
Navigation  Department 


V 


NORTHROP 


Electronics  Division 


Volume  VI 
of  the 

AN/BRN-7  OMEGA  COMPUTER 
PROGRAM  SPECIFICATION 


Volume 


Performance  Specification 
Design  Specification 
Synchronization  Subprogram  Design 
OMEGA  Processing  Subprogram  Design 
Tracking  Filter  Subprogram  Design 
Kalman  Filter  Subprogram  Design 
Propagation  Prediction  Subprogram  Design 
Navigation  Subprogram  Design 
Executive  Subprogram  Design 
Control-Indicator  Subprogram  Design 
Built-in  Test  Subprogram  Design 
Common  Subroutines  Subprogram  Design 
Appendix 


Vol  VI 


NORT  73-48 


W 


NORTHROP 


Electronics  Division 


CONTENTS 

Section  Pa rc 

1 SCOPE  1 

1.1  Identification  1 

1.2  Kalman  Filter  Subprogram  Tasks  1 

1.2.1  Introduction  1 

1.2.2  Kalman  Task  Summary  4 

2 APPLICABLE  DOCUMENTS  7 

3 REQUIREMENTS  8 

3.1  Detailed  Description  8 

3.1.1  Reference  Labels  to  Flow  Diagrams  8 

3.1.2  Description  of.  Flow  Diagrams  8 

3.2  Flow  Diagrams  34 

3.3  Computer  Subprogram  Environment  85 

3.3.1  Kalman  Filter  Tables  85 

3.3.2  Temporary  Storage  85 

3.3.3  Input/Output  Formats  85 

3.3.4  Required  System  Library  Subroutines  86 


Vol  VI 


ii 


NORT  73-48 


NORTHROP 


Electronics  Division 


SECTION  1 
SCOPE 


l . 1 IDENTIFICATION 

Volume  I,  Submarine  OMEGA  Computer  Program  Performance  Specification,  defines 
the  functional  requirements  for  the  Submarine  OMEGA  Computer  Program  which  is 
used  by  the  AN/BRM-7  OMEGA  Navigation  Set.  The  Navigation  set  and  the  OMEGA 
program  together  comprise  the  Submarine  OMEGA  Navigation  System.  The  tape 
which  defines  the  computer  program  is  entitled  AN/BRN-7  Navigation  Program 

Volume  II,  Submarine  OMEGA  Computer  Program  Design  Specification,  allocates 
the  functional  requirements  of  Volume  I to  the  computer  routine  and  sub- 
program level. 

This  volume  describes  the  subprogram  designated  as  Kalman  Filter  which  has 
the  abbreviation  CF  in  the  program  listing  (Volume  XIII).  Note  that  in  all 
the  thirteen  volumes  which  comprise  the  Submarine  Omega  Computer  Program 
Specification  the  designators  Combinational  Filter  and  Kalman  Filter  are 
interchangeable. 


1.2  KALMAN  FILTER  SUBPROGRAM  TASKS 

The  outputs  of  the  tracking  filters  are  well-filtered  values  of  phase- 

2 

difference,  9> , along  with  the  estimates  of  phase  variance,  a and  phase 

2 ^ ^ 

rate  variance  CT  . . . It  is  within  the  combinational  filter  operations  that 

the  outputs  of  the  tracking  filters  are  statistically,  optimally  combined, 
arriving  at  "best"  estimates  of  system  position  and  velocity.  The 
combinational  filter  also  performs  the  coordinate  conversion  from  phase 
difference  to  geodetic  coordinates  (latitude  and  longitude).  The  rate  of 
change  of  frequency  between  the  local  oscillator  in  the  receiver  and  trans- 
mitted OMEGA  signals  is  also  determined.  The  combinational  filter  is  also 
used  for  lane  determination.  Lane  determination  (laning)  is  accomplished 
by  use  of  a multiple  state  vector  technique. 


1.2.1  Introduction 

a)  The  combination  filter  is  a Kalman  filter.  The  Kalman  approach  to 
filtering  and  prediction  can  be  described  as  a linear,  recursive, 
minimum  variance  filter.  The  fundamental  concepts  involved  are  those 
of  state,  state  transition,  measurement,  and  optimal  weighting.  The 
states  of  the  filter  are  differentials  of  system  parameters;  i.e. , 
error  in  position,  error  in  velocity,  etc.  The  states  of  the  system 
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are  described  by  the  solution  of  linear  vector  difference  equations. 

The  equations  upon  which  filter  operations  are  predicated  are: 

1)  X(K)  = $ (K,  K-l)  X (K-l)  + V (K) 

2)  Y (K)  = M(K)  X (K)  + U(K) 

where  X(K)  is  the  vector  state  of  the  system  error  (Table  1) 

3)  ^(K,  K-l)  The  transition  matrix. 

4)  V (K)  system  forcing  function  (for  Kalman  operations  a white 

noise  sequence) 

5)  Y(K)  A vector  of  state  observables  (measurement) 

6)  M(K)  A state  transformation  matrices  (Extraction  matrix) 

7)  U(K)  Measurement  noise  (white  noise  sequence) 

b)  The  Kalman  technique  linearly  combines  the  previous  estimate  X with 
the  measurement  to  arrive  at'  a minimum  variance  estimate  which  is 
then  time  updated  until  the  next  measurement.  The  measurement  operation 
is  best  described  by: 

£(K/K)  = £(K/K-1)  + b(K)  [Y(K)  - M(K)  & (K/K-l)] 

A , v 

where:  X(K/K)  new  estimate  of  state  at  time  K based  on  K measurements 

A 

X(K/K-1)  Time  updated  prediction  of  previous  estimate  at  time  K 
based  on  K-l  measurements 

b(K)  optimal  weighting  vector 

Optimal  weighting  is  determined  by  means  of  recursive  techniques  based 
upon  obtaining  minimum  error  variances.  This  weighting  is  a function 
of  the  covariance  of  the  difference  between  system  error  state  and  esti- 
mation of  state;  i.e.: 

P = E [(X-$)  (X-$)T] 

The  combinational  filter  is,  therefore,  capable  of  computing  optimal 
estimates  of  position  and  velocity  along  with  other  elements  of  its 
state  vector.  The  filter  utilizes  two  types  of  observations.  One  is 
the  difference  between  the  phase  difference  of  the  tracking  filter  and 
a value  of  phase  difference  based  upon  the  combinational  filter's  time- 
updated  position.  The  other  observation  consists  of  a received 
correction  to  position  entered  through  the  C & I Panel.  B lsed  on  these 
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measurements,  the  filter  computes  optimal  estimates  of;  position  errors, 
driving  velocity  errors,  and  oscillator  drift.  The  recursive  formulation 
of  filter  operations  permits  each  measurement  to  be  processed  and  then 
discarded.  In  this  fashion  the  values  of  previous  measurements  do  not  have 
to  be  stored  in  the  computer,  since  all  the  information  is  contained  within 
the  state  estimate  vector  and  covariance  matrix. 

c)  The  uncertainty  in  position  error  can  produce  an  ambiguity  in  lane 
count,  the  number  of  wave  lengths  between  the  transmitting  station 
and  the  receiver.  This  ambiguity  exists  primarily  when  the  system 
is  first  turned  on.  To  resolve  this  lane  ambiguity,  a multiplicity 
of  state  vector  estimates  X^is  carried  by  the  filter.  These  X£ 
correspond  to  the  several  possible  lanes  or  integral  values  of  phase 
measurement.  As  measurements  corresponding  to  the  three  different 
frequencies  are  taken  the  ambiguity  in  lane  count  is  reduced.  Measure- 
ments from  different  stations  tend  to  further  reduce  the  lane 
uncertainty.  The  overall  effect  is  a reduction  in  The  number  of  sta^e 
vector  estimates  Xj^  that  can  be  considered  reasonable  estimates  of  the 
true  state  vector  until  only  one  reasonable  estimate  remains.  The 
criterion  for  reasonableness  is  based  upon  variance  considerations. 

An  average  of  the  Xjj,  X^VG»  is  also  computed. 

d)  The  error  vector  has  a rank  of  nine  to  allow  for  the  possible  error 
sources  in  various  OMEGA  mechanizations.  In  the  phase  difference 
approach  five  of  the  nine  elements  are  utilized. 

e)  The  estimates  described  above  are  treated  computationally  in  the  com- 
binational filter  as  elements  of  a vector  (=  error  state  estimate 
vector  = state  vector  = estimator  = X). 

f)  Before  the  intra-cycle  operations  are  described  mathematically,  the 

vectors  and  matrices  which  are  required  to  carry  out  these  operations 

will  be  defined. 

1)  State  vector  (X:  9x1) 

2)  Transition  matrix  ($:  9x9) 

3)  Covariance  matrix  (P:  9x9,  symetric  and  positive  semi- 

definite)  ' ' 

4)  System  Noise  Matrix  (R:  9x9  and  diagonal) 

5)  Measurement  (Y:  lxl) 

6)  Measurement  Matrix  (M:  1x9) 


Vol  VI 


3 


NORT  73-48 


NORTHROP 


Electronics  Division 


7) 

Measurement  Noise  (C: 

1 x 1) 

8) 

Measurement -residual 

(Res;  1 

x 1) 

9) 

Measurement -residual 

variance 

(V: 

1 

10) 

Optimum  weighting  vector  (b; 

9 x 

1) 

ID 

Epsilon  (e  : lxl) 

g)  Interfacing  requirements  with  other  subprograms  have  been  touched  upon 
above.  A restatement  and  summary  will  aid  in  clarification  of  the 
operations  performed.  The  subprograms  that  will  interface  with  the 
combinational  filter  include; 

1)  Navigation 

2)  Propagation  Prediction 

3)  Tracking  Filters 

4)  Control  & Indicator 

The  interfacing  with  the  first  routine  includes  mode  corrections  to 
system  position,  and  corrected  system  velocities.  Interfacing  with 
the  last  routine,  in  addition  to  display  functions,  includes  position 
correction  measurement  residual  data.  The  tracking  filter  supplies  0, 
a2  _2 . . 

00  and  00  for  a particular  station/frequency.  Knowledge  of  com- 
binational filter  utilizations  and  resetting  of  tracker  filter  para- 
meters is  required  for  this  particular  interface.  The  interfacing 
with  the  propagation  routine  includes  the  C element  and  0 corrections 
for  the  particular  station/frequency . 


1.2.2  Kalman  Task  Summary 

The  overview  of  the  Kalman  Subprogram  is  given  in  Figure  1.  The  following 
represent  the  tasks  performed  by  the  subprogram. 

a)  Initialization 

Initialization  occurs  after  synchronization  and  whenever  the  initial 
time  or  position  is  entered  by  the  operator  if  the  entry  occurs  after 
synchronization.  Initialization  consists  of  setting  the  Process,  NO 
KICK,  and  Kalman  Started  markers;  setting  the  Station  Check  and  state 
vector  counters  to  zero,  and  inializing  the  covariance  matrix  (P) , the 
transition  matrix  ($),  the  noise  matrix  (R)  and  the  first  state  vector 
(X). 
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TIME  UPDATE  COVARIANCE  MATRIX 
TIME  UPDATE  ALL  STATE  VECTORS 
UPDATE  POSITION  MATRIX 


NO 


PERFORM  POSITION  MEASUREMENT  ON 
NORTH  AND  EAST  ERRORS 


TRACKING  FILTER  ME AS 
UREMENT 


NO 


II 

r K EXIT 

EXIT 


BBK 


FIGURE  1 
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b)  Time  Update 

The  operations  consist  of  time  update  of  the  state  vector  (X)  and 
covariance  (P)  matrices.  This  requires  the  use  of  the  transition 
matrix  $ which  will  mathematically  propagate  the  expected  errors 
across  the  time  interval  since  the  last  update. 

c)  Measurement  Update 

There  are  two  measurement  types  available  and,  therefore,  generated 
prior  to  measurement  update.  They  are:  1)  Tracking  filter  estimate 
of  the  phase  of  a station/i requency  combination,  and  2)  Navigator  in- 
serted position  correction.  For  the  first  type,  the  measurement  is 
utilized  for  estimation  of  the  entire  system  error  model  while  the 
second  type  is  utilized  for  resetting  of  position  error  in  accordance 
with  navigator  indication  of  quality  fix.  For  each  measurement  type 
however,  the  operations  are  similar  and  consist  of  generation  of  the 

measurement  residual,  Res.,  extraction  matrix,  M.  and  measurement 

1 1 

confidence  scalar,  C.. 
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SECTION  2 

APPLICABLE  DOCUMENTS 

a)  Submarine  OMEGA  Computer  Program  Performance  Specification  (Volume  1 
of  the  Submarine  OMEGA  Computer  Program  Specification) 

Applicable  Sections 

3.1  Introduction 

3.2  Functional  Description 
3.2.5  Detailed  System  Operations 

3.3  Detailed  Functional  Requirements 
3.3.2  Signal  Input  Timing 

3.3.9  Phase  Difference  Processing 

3.3.10  Tracking  Filters 

3.3.11  Combination  Filter 

3.3.12  Propagation  Prediction 

3.3.13  Velocity  and  Heading  Processing 

b)  Submarine  OMEGA  Computer  Program  Design  Specification  (Volume  II  of  the 
Submarine  OMEGA  Computer  Program  Specification). 

c)  NORT  71-41,  NDC  1070  MACRO  ASSEMBLER,  MAY  1971 

d)  NORT  68-115A,  Detailed  Description  of  NDC-1070  Computer  Instructions, 
Revision  A,  February  1970. 

e)  NORT  69-87A,  NDC-1070  Flow  Chart  Program,  User's  Manual. 
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SECTION  3 
REQUIREMENTS 


In  order  to  understand  the  program  description  contained  in  the  following 
pages,  it  is  necessary  that  the  reader  will  have  become  familiar  with  the 
associated  functional  requirements  found  in  Volume  I,  Performance  Specifi- 
cation, and  with  the  subprogram  allocation  found  in  Volume  II,  Design 
Specification . 


3.1  DETAILED  DESCRIPTION 

3.1.1  Reference  Labels  to  Flow  Diagrams 

The  code  used  to  reference  the  particular  block  in  the  flow  diagrams, 
Section  3.2,  is  as  follows:  The  first  number,  preceded  by  a p,  is  the 
page  number  found  in  the  upper  right  corner  of  the  diagrams.  This  will  be 
followed  by  a slash  sign  (/)  to  separate  the  page  number  from  the  block 
designator.  The  designator  will  either  be  a mnemonic  label  (e.g.,  TEST 
SYNC),  a local  label  indicated  by  a dollar  sign  ($) , or  an  integer.  The 
two  types  of  labels  reference  the  particular  information  block,  on  the 
given  page,  to  which  the  label  is  attached.  The  integer  number,  n,  means 
that  the  referenced  block  is  the  n*-^  block  from  the  top  of  the  page. 

For  instance,  p8/3  would  refer  to  page  8 and  the  third  information  designa- 
tion down;  The  label  pl/$2+3  refers  to  page  1,  and  the  3r<^  information 
block  after  the  label  $2;  p2/7,8,9  refers  to  page  2 and  the  7t*1,  and 

9*-^  blocks. 


3.1.2  Description  of  Flow  Diagram 
3. 1.2.1  Initialization 
Pl/Kalman  Filter  to  p2/l 

The  next  Q-task  is  END  BURST  and  will  occur  in  5 milliseconds.  The 
Combinational  Filter  is  entered  5 milliseconds  prior  to  the  End  Burst 
marker  on  every  station.  However,  only  at  a station  A transmission  on 

10.2  kHz  will  the  filter  be  sequenced. 
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P2/  2 through  P3/3 

Initialization  occurs  after  synchronization  and  whenever  the  initial  time 
or  position  is  entered  by  the  operator  if  the  entry  occurs  after  synchroni- 
zation. Initialization  consists  of: 

STATE  VECTOR  Counter,  N = 0 (represents  1 state  vector) 

State  vector:  Initialize  number  1 to  0. 

Covariance  matrix  = 0. 

P11  = P22  = inserted  Position  quality  if  position  has  been  entered. 

P^  and  ?22  are  set  equal  to  the  values  existing  when  the  system  was 
turned  off. 

P^  and  P^2  are  set  equal  to  a position  quality  of  (0  n mi)  if  time  has 
been  entered  and  position  has  not  been  entered. 

2 

P. , = (0.25  microsec/microsec)  . 


2 2 

a , , = (5  n mi)  or  P. , , whichever  is  largest, 

kick  ii 


Kalman  Started  marker  = true 


R„  matrix  saved 


When  the  PHI  matrix  is  separated  into  a modified  PHI  plus  an  identity  matrix 
then  the  elements  of  the  modified  PHI  are  as  follows: 

¥>(1,8)  = ¥»(  2,9)  = At/Ro 
0(3,4)  = At 

0(8,8)  = 0(9,9)  = -flvAt 

This  concept  is  further  described  in  the  description  of  the  PHI  P PHIT 
subroutine  on  page  27  of  flow  diagram.  The  elements  of  modified  PHI  are 
stored  in  14  words,  some  of  which  are  zero. 
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The  system  noise  matrix,  R,  is  zero,  except  for 
R(8,8)  = R (9 , 9 ) = 2o^av  RvAt. 


°NAV=3  ShipS11°8 
10  manual 

20  NO  RATE  Aiding 


B = 
v 


sec 


-1 


.05(3600) 


3. 1.2. 2 Time  Update 


The  operations  consist  of  time  update  of  the  X and  P matrices.  The 
equations  utilized  are: 

A A 

1)  X = JX 

2)  P = J>P$T  + R 


Both  these  equations  require  the  use  of 
mathematically  propagates  errors  across 
update.  The  transition  matrix  is  shown 


the  transition  matrix  $ which 
the  time  interval  since  the  last 
in  Table  1. 


The  transition  matrix  provides  only  the  propagation  of  predictable 
(i.e.,  deterministic)  elements  of  X and  P across  the  computational  interval 
At  . Propagation  of  the  random  effects  across  this  interval  is  accomplished 
by  means  of  the  additive  diagonal  matrix  R.  Table  2 specifies  the  diagonal 
elements  of  R by  navigation  mode. 


Investigation  of  the  elements  in  $ and  R reveals  the  dependence  of  time 
between  updates  as  a factor  in  computation.  This  update  should  be  done 
at  a constant  rate  to  minimize  computational  cost.  Due  to  the  dynamical 
errors  associated  with  maneuver  and  the  utilization  of  velocity  error 
elements  as  output  information,  it  is  felt  that  this  iteration  time  should 
be  a maximum  of  30  seconds.  The  iteration  time  chosen  is  the  natural  one 
of  10  seconds. 


P3/TIME  UPDATE  (P)  through  P4/3 

The  subroutine  PHI  P PHIT  is  used  twice  to  compute 
P(K)  = $ (K,  K-l)  P(K-l)  5 (K,  K-1)T  + R (K) 
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TABLE  1 TRANSITION  MATRIX 


• 

-**3 

S02 

t 

o 

t 

o 

P1 

P2 

P3 

SV2 

8V3 

-S*3 

1 

At 

R 

o 

502 

1 

H 

fco 

D 

D 

• 

Co 

1 

P1 

0 

P2 

0 

P3 

t 

5V2 

1-^yAt 

5V3 

1-^yAt 

. 


A 
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TABLE  2 SYSTEM  NOISE  MATRIX 


ELEMENT  NO. 

EXPRESSION 

(1,  1) 

0 

(2,  2) 

0 

(3,  3) 

0 

(4,  4) 

0 

(5,  5) 

0 

(6,  6) 

0 

(7,  7) 

0 

(8,  8) 

2ctnavbv  At 

(9,  9) 

2ctnavbv  At 

NOTE:  All  off-diagonal  terms  are  zero. 
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P4/TIME  UPDATE  X to  page  4 bottom 

During  most  of  the  cruise  there  will  be  only  one  state  vector.  However, 
under  adverse  conditions,  the  variances  increase  and  the  OMEGA  system 
becomes  uncertain  about  the  position  estimate.  Under  these  conditions 
there  will  be  a number  of  state  vectors  generated,  each  of  which  may  be 
the  vector  representing  the  true  position  and  must  be  maintained  until 
that  time  that  the  variances  decrease. 

In  the  entry  block  of  the  loop  the  average  state  vector  is  initialized. 


P4/$6, 

The  variable  XX  represents  one  of  the  9-element  state  vectors  which  is 
assembled  from  component  parts.  Becase  the  last  five  elements  of  the 
multiple  state  vectors  are  identical  only  the  first  four  are  saved  for 
each  of  the  multiple  state  vectors.  Consequently,  when  each  is  time 
updated  it  must  be  assembled  with  the  last  5 components. 


P4/$6+2 

Xk  (K)  = $(K,  K-l)  Xk  (K-l) 

P4/$6+4 

The  combinational  filter  generates  new  state  vectors  based  upon  the  values 
of  the  residuals  computed  for  a given  tracking  filter  output  (corresponding 
to  a given  station  difference  and  frequency  pair).  An  average  state  vector 
is  also  computed  by  computing  an  element- bv-element  average  over  the  state 
vectors  generated: 

✓\  /> 

* A X - X 

X = X + k avg 

avg  avg  # state  vectors 

The  average  variance  is  also  computed.  See  p33,  AVERAGE  X subroutine. 


P5/1.2 

The  scatter  vector  is  a 9-element  matrix,  each  element  of  which  represents 
the  variance  of  the  error  estimates  of  the  same  respective  element  of  the 
multiple  state  vectors. 
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P5/2.3.4 


If  the  position  scatter  error 
error  elements  of  the  average 
correct  the  position  via  the  R 


is  less  than  4.76  n miles  then  the  position 

state  vector  XXX(l),  XXX(2)  are  used  to 

. , matrix . 
ij 


P5/$7 

Each  state  vector  is  now  corrected  by  deleting  the  correction  just  made  to 

the  R. . matrix, 
ij 

P5/$7+3 

AV.  = AV.  - p AV.AT  i = 1,2 

1 1 v 1 

P6/1-8 


This  page  manages  the  updating  of  AV^  with  respect  to  the  NAV  Mode  used. 


P7/1.2 

The  system  is  confirmed  when  all  three  frequencies  from  three  stations  are 
being  received. 


P7 / $52  through  P7/$53 

Routine  maintenance  of  covariance  diagonal. 


P7/$53+l 

Time  increase 


dt 


P8 

Routine  Maintenance  of  OMEGA  clock.  When  above  integral  accumulates  to 
5 msec,  then  one  interrupt  is  added  to  or  subtracted  from  the  computer 
time.  See  Volume  IV,  OMEGA  Processing  Subprogram  Design,  END  SLOT  routine. 
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3. 1.2. 3 Measurement  Update 

As  stated  in  the  task  description  there  are  two  types  of  measurement 
available;  Position  Fix  from  the  operator,  and  a Tracking  Filter  measure- 
ment. Each  is  processed  with  a different  assumption;  with  a position  fix 
the  computer  program  assumes  the  OMEGA  operator  is  correct,  with  a tracking 
filter  fix  the  software  will  attempt  to  compromise  and  resolve  differences. 


a)  Position  Fix  Measurement  Update 


After  setting  up  arguments  for  position  fix  the  COMPUTE  BI  & C subroutine 
is  entered  to  compute 

Q. (K)  = M.  P(K)M.T  + C. 

1 111 

»i-  KT  + "iT ']  [Q_1] 

where  b^  is  the  weighting  vector 

M.  is  the  Measurement  matrix 
i 

P is  the  covariance  matrix 
e is  the  divergence  control  factor 
C.  is  the  measurement  variance. 

l 

Following  the  calculations  for  the  weighting  vector  the  MEASUREMENT  UPDATE 
X subroutine  is  called.  To  fully  understand  the  measurement  updating  pro- 
cess, refer  to  the  description  of  the  MEASUREMENT  UPDATE  X subroutine,  flow 
diagram  page  36.  It  is  possible  that  a state  vector  has  not  been  generated. 
The  overall  sequence  of  the  MEASUREMENT  UPDATE  X subroutine  is  shown  in 
Figure  2. 


P10/4 

The  measurement  is  unsuccessful  due  to  either  the  first  (N)  or  second  (E) 
component  of  position  error.  In  either  case  the  Flag  B is  set  true  to 
bypass  the  36-mile  limit  on  position  error,  which  is  part  of  the  new  state 
vector  acceptability  check. 
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PlO/5,6 

If  the  unsuccessful  measurement  was  due  to  the  second  component  of  position 
error  then  return  to  the  measurement  update  subroutine  and  try  again. 

If  due  to  a first  component  (N)  then  reset  arguments  for  the  second  and  try 
aga in . 


P10/7  through  Pll 

The  measurement  was  successful.  If  there  is  another  component  to  process 
then  return  to  the  measurement  update  subroutine  and  process  the  second 
component.  If  not  then  establish  sigma  KICK  by  performing  in  order: 

1)  S (MAX)  = max  of  (Pn>  oRICK) 

2)  “kick-  "in  <S<M4X>,  V' 

where  C is  the  position  variance  used, 
pos 

3)  °KICK  = ro3*  of  ^kick’  5 n miles)- 


b)  Tracking  Filter  Measurement  Update 

The  tracking  filter  measurements  occur  at  an  asynchronous  rate.  Availability 
of  a measurement  is  indicated  by  the  n counter  of  the  tracking  filter,  as 
described  in  Volume  V,  Tracking  Filter  Subprogram  Design. 

When  a tracking  filter  measurement  for  some  phase-difference  pair  is 
accepted,  an  estimate  of  that  measurement  is  computed.  This  computed 
estimate  is  based  upon  the  present  position  as  indicated  by  the  Navigation 
Routine  and  propagation  corrections  determined  by  the  Propagation  Routine. 

The  measurement  Y.  is  the  difference  between  this  computed  phase  estimate 

and  the  phase  estimate  from  the  tracking  filter. 


For  each  measurement,  the  measurement  residual  is  computed.  The  measurement 
residual  is  the  difference  between  the  measurement  and  its  predicted 

value  M^X.  The  measurement  matrix  M^^  consists  of  the  algebraic  relationships 

between  the  measurement  and  the  error  state  X.  The  elements  of  M.  are 

l 

computed  within  this  routine.  The  remaining  parameter,  measurement  noise, 

2 

is  an  addition  of  cr  ,,  and  the  confidence  figure  from  the  propagation 
routine  associated  with  its  computations. 
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After  the  time-update  of  the  state  vector  and  the  covariance  matrix,  the 
tracking  filter  outputs  are  scanned  station-pair  by  station-pair , starting 
with  the  station-pair  immediately  following  the  last  used  by  the  combina- 
tional filter.  As  mentioned  above  a tracking  filter  output  is  ready  to  be 
processed  by  the  counter  criterion. 

When  a station  is  processed,  all  frequencies  meeting  the  readiness  criteria 
are  processed  sequentially.  When  a tracking  filter  has  been  read,  the 
combinational  filter  resets  the  tracking  filter  variances  to  their  velocity- 
mode-dependent initial  values.  If  no  tracking  filters  are  ready  for  use  by 
the  combinational  filter,  the  routine  exits. 

P12 /TRACKING  FILTER  MEASUREMENT 

The  search  for  a good  measurement  to  process  in  the  combinational  filter 
begins  with  the  station  used  last.  The  Difference  Frequency  Marker  is  set 
and  may  be  reset  later. 

P12 /$! 3 through  P13/$63+2 

Checks  the  size  of  P^  and  P^  + ?22'  This  is  a programming  convenience 
in  the  case  that  P ^ overflows  the  register  resulting  in  a small 

remainder  in  the  register. 

P13/$12  through  P13/$l 

An  additional  constraint  on  the  station  selection  scheme  exists  when  the 
variance  on  position  error  as  carried  in  the  covariance  matrix  exceed 

2 

4 n miles  squared  [P  + (4  n miles)  ].  When  this  occurs,  two 

frequencies  from  a given  station  must  be  available  for  processing.  This 
constraint  holds  down  the  number  of  extraneous  state  vectors  generated 
when  a large  uncertainty  in  position  exists.  A measurement  generated 
under  these  conditions  consists  of  the  difference  of  the  measurements  (Y. ) 
of  the  two  frequencies.  The  measurement  matrix  (M)  also  is  computed  by 
differencing  the  M^  for  the  two  frequencies.  Thus  only  one  measurement 

is  generated  from  the  two  tracking  filter  outputs. 

The  frequency- need  counter,  P13/$l,  is  set  to  2 if  the  difference 
frequency  mode  is  used,  and  1 if  not. 
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PI  3/ $3 

If  the  station  under  scrutiny  has  been  disabled  by  the  operator  the 
sequencing  will  branch  to  Pl4/$2.  The  check  on  the  number  of  frequencies 
will  return  sequencing  to  this  same  test,  i.e.,  P13/$3.  The  third  time 
through  this  loop  will  find  the  program  at  P14/$2  + 1 where  a NO  branch 
results  in  Pl4/$28,  ending  the  check  on  this  station  and  returning  to 
P12/$13  for  another  station  to  check. 

P14/1 ,2,3 

If  the  station  has  not  been  disabled  then  the  N counter  is  checked.  If 
M>3  then  the  frequency- need  counter  is  decremented  by  1.  See  P13/$12 
description  for  setting  the  frequency- need  counter.  The  frequency- need 
counter  is  permitted  to  go  negative. 


P14/$2 


Until  all  three  frequencies  have  been  checked  for  this  station  the 
sequencing  will  branch  back  to  P13/$3. 


P14/$2-fl 

After  all  three  frequencies  have  been  checked  there  must  be  at  least  one 
usable  frequency,  or  two  if  in  the  difference  frequency  mode.  If  so, 
continue;  if  not,  then  the  station  is  not  usable  (PI 5 / $28 ) . 


P14/6 


Submarine  OMEGA  does  not  use  rho-rho  so  continue. 


P14/7 , P15/1 

If  the  measurement  is  not  from  the  base  station  then  it  is  an  available 
measurement.  If  it  is  from  the  base  station  then  to  be  classified  as 
an  available  measurement  the  last  available  measurement  must  not  have 
been  from  the  base  station. 


P15/$28  through  P15/$28+2 

If  all  stations  have  been  checked  then  no  measurements  are  processed.  If 
not  then  return  to  P12/$13  and  select  another  station. 

Measurement  Available 
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P15/$4  through  P 1 6 / $ 7 1 

Setup  for  measurement  processing. 

P 1 6 / $ 7 through  P16/$7+5 

o 

If  a.,  >.031  then  the  corresponding  data  is  not  classified  as  a good 
vt> 

measurement  and  the  corresponding  marker  is  set  false;  otherwise  it  is 
set  true. 


PI  7 / $42+4 , 5 


rll  ril 


(earth's  polar  radius) 

(earth's  equatorial  radius)^ 


► -1  R,  X S 

tan  I 1 


-1  'J  (R^  X "sj  * (R L X "s) 


'l  "1 


tan 


-1 


[(r,„s 


12  3 


rl3S2)  + (r13Sl 


r 11s  3>  + (rllS2 


r12Sl)  1 


rllsl  + r12s2  + r13S3 


The  components  of  the  transmitter  location  vectors  (station  and  base) 

S must  be  in  geocentric  (not  geodetic)  coordinates.  The  vector  R^  is 

the  first  row  of  the  R. . Matrix  modified  by  the  first  equation  to 
^ ij 

change  R^  from  geodetic  to  geocentric  coordinates. 


P17/6 


This  branch  insures  that  base  station  data  and  station  data  will  always 
be  processed  together. 

PI  7/7 , 8 , 9 

If  station  is  too  close  or  too  far  it  is  not  usable  due  to  the  lack 
of  interf requency  coherency.  Otherwise  the  measurement  loop  is  initiated. 
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P20/$50  through  P20/$50+5 

The  subroutine  THETAC  is  used  twice  to  calculate  the  following  for  either 
station-to-base  or  base-to-base  measurements. 

Let  0 . = predicted  phase  for  station 
ci 

= predicted  phase  for  base  station 

0^  = estimated  phase  for  base-station 

t.-t,  = difference  in  time  between  measurements. 

1 k 


then 


0’ . = 0 . - 0 , + (t . - t,  ) 0k 
ci  ci  ck  i k -p- 


P2 1 / $5 1 

The  measurement,  Y,  and  the  epsilon  factor,  e , are  now  calculated  where 
?.k  is  the  measured  phase  from  the  tracking  filter. 

Because  of  the  utilization  of  fixed  point  limited  word  lenth  arithmetic 
for  all  internal  computations,  an  epsilon  factor,  e,  is  specified  to 
prevent  divergence  of  the  estimation  process.  This  divergence  can  result 
from  the  buildup  of  arithmetic  round-off  and  truncation  errors  leading 
to  loss  of  positive  semidefiniteness  of  P.  An  additional  use  of  the 
epsilon  factor  is  to  reduce  mismodeling  effects. 

Y = 0.,  - 0'  . 
i ik  ci 


el  = e2 


C. 

i 

16X2 


16  V, 


where  V is  the  velocity  of  propagation;  a constant. 
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P2l/$51+2,3 

If  the  combinational  filter  is  in  the  difference  frequency  mode  then  for 
the  first  frequency  the  program 

I 

1)  Saves  M,  , M„,  M,  and  M . ; 6..  , C.  and  the  KICK  LIMIT  Q.  (See  the 

12  3 4 lk  l i 

Compute  BI  and  C subroutine). 

2)  Resets  this  tracking  filter  by  setting  n-counter  to  zero. 

1_ 

12 

0 

(0.006tt)2 


3)  Complements  Difference  Frequency  Marker  and  updates  the  Dump  counts 
for  this  station- frequency . 

4)  Branches  to  P18/$ll  where  the  2nd  frequency  will  be  initiated  by 
calculating  MgTATI0N  - 

Eventually  this  same  check  will  take  the  "NO"  branch  to  process  the 
second  frequency  where  the  variables  of  item  1)  above  are  differenced 
with  those  of  the  second  frequency. 

P21 / $90 

The  weighting  matrix,  b^  and  the  kick  limit,  Q , are  calculated  using 
the  compute  BI  and  C subroutine. 

P21/$90+l 


As  indicated  beneath  this  information  block  and  in  Figure  2 (under  des- 
cription P9/P0SITI0N  MEASUREMENT),  if  this  was  considered  a good  measure- 
2 2 

ment,  i.e.,  a < (0.00031  cycle/sec)  and  no  state  vectors  were  generated, 

then  sequencing  exits  the  Combinational  Filter.  However,  if  not  a good 
measurement  the  sequencing  will  return  to  P23/$10  to  see  if  there  are  more 
frequencies  to  process. 
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P22/1 ,23 

To  continue  sequencing  at  this  point  a new  state  vector  must  have  been 
generated.  If  more  than  one  state  vector  was  generated,  then  the  Station 
Check  counters  are  zeroed.  If  only  one  state  vector,  then  a bit  for  this 
station  and  frequency  is  posted  in  the  appropriate  location. 


P2  2 / $ 7 7 


Tracking  Filters  reset  as  indicated  in  item  1)  under  description  P2I/$5]+2,3, 
unless  it  is  the  base  station, in  which  case  it  is  never  reset. 

P23/ $65- $97 


The  difference  frequency  marker  is  set  or  reset  and  the  Kalman  dump  counter 
is  incremented  for  this  station-frequency. 


P24/1 . $92 


If  there  are  more  frequencies  then  the  program  branches  to  the  beginning 
of  the  measurement  loop.  If  not  then  branch  to  P25/M  EXIT. 


P24/$91 

Discussed  under  P21/$51+2,3 


P25 

The  NO  KICK  Marker  is  set  true  if  the  system  is  confirmed,  the  KALMAN  IN 
PROCESS  Marker  is  reset. 

Preparations  are  made  for  Propagation  Prediction  which  is  used  here  as 
a subroutine  to  insure  that  it  immediately  follows  the  processing  of  the 
Combinational  Filter  and  on  the  same  processing  level. 


3. 1.2. 4 Kalman  Subroutines 

The  following  are  subroutines  used  by  the  Kalman  Subprogram  only.  The  flow 
diagram  pages  upon  which  the  subroutines  can  be  located  are  given  in 
parenthesis . 

a)  Initialize  P5,  P6,  P7  (page  26) 

The  routine  initializes  the  5th,  6th  and  7th  rows  and  columns  of  the 
P matrix.  Sigma  (P (I ) in  P26/4  refer  to  constants. 
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b)  Phi  P Phi  Transpose  (pages  27-30) 

This  subroutine  is  designed  to  multiply  PHI  by  a matrix  Z: 

K = (Z)  (PHI)T 

where  Z=  X,  a 1 x 9 matrix 

= P,  a 9 x 9 matrix 
T 

= P , a 9 x 9 matrix 
The  result  replaces  Z. 

The  introductory  remarks  to  the  subroutine  indicate  14  non-zero  elements 
in  the  PHI  matrix.  This  is  not  true  and  is  a leftover  from  previous 
modifications.  The  14  elements  remain,  some  of  which  have  been  zeroed. 

The  transition  matrix  is  as  indicated  in  Table  1.  However,  it  is  multipled 
as  if  the  unit  elements  were  not  present.  In  other  words  the  phi  matrix  is 
separated  into  the  sum  of  a modified  PHI  matrix  and  the  identity  matrix. 
After  forming  the  product  of  P,  PT  or  X and  the  modified  PHI  matrix  the 
subroutine  will  then  concentrate  on  the  product  with  the  identity  matrix. 


P27/PHI  P PHIT 

When  the  Z matrix  represents  P,  then  each  element  of  Z is  defined  by  two 
words,  i.e.,  P is  a double  precision  matrix.  The  elements  of  Z=P  are 
stored  linearly  in  the  computer  as  P^,  P^,  ...  P^g,  •••  P29’ 

P , P99’  each  element  defined  by  two  words.  P is  indexed  by  an 

increment  of  2 with  a limit,  value  of  18.  These  indices  are  represented  by 
I and  J in  the  flow  diagrams. 

T 

When  Z represents  P the  index  I = 18  and  J = 2. 

When  describing  mathematically  the  manipulation  of  the  matrix  the  fictional 
index  M is  used  which  increments  each  element  of  a P row  by  one  as  would 
be  expected.  The  counter  K tracks  the  row  number  of  Z.  N tracks  the 
column  vector  of  PHI. 


P27/$6 


The  TEMP  vector  is  a temporary  row  vector 
when  completed,  will  replace  the  same  row 


of  the  product  matrix  which, 
of  the  multiplier  matrix. 
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P27/$5, $3 

The  PHI  pointer  indexes  the  non-zero  elements  of  PHI.  These  14  elements 
are  as  indicated  in  the  introductory  remarks  on  page  27.  Admittedly 
some  are  zero.  However,  in  previous  program  modifications  they  were  non- 
zero, and  rather  than  change  the  program  sequencing  it  was  expedient  to 
zero  those  elements  no  longer  used. 

The  pointer  will  index  through  the  14  elements  and  use  them  one  by  one 
when  indicated  by  the  PHI  CODE  matrix.  This  is  a 9 x 9 binary  matrix 
which  is  defined  by  9 9-bit  registers.  A zero  bit  in  a particular 
register  indicates  that  there  is  a non-zero  element  of  the  modified  PHI 
matrix,  in  the  same  relative  position  as  the  PHI  CODE  element,  to  be 
multiplied  by  something.  Because  of  the  ordering  of  these  14  elements 
the  PHI  pointer  always  indicates  the  proper  element  to  be  used  as  the 
multiplicand.  Succeeding  the  multiplication  the  pointer  is 
incremented.  Conversely,  a one  bit  in  a register  indicates  a zero 
element  of  the  modified  PHI  matrix  and  no  multiplication  takes  place. 

This  method  reduces  the  number  of  products  in  a 9 x 9 matrix 

from  729  to  a more  reasonable  number.  These  operations  are  explained 

in  subsequent  steps. 


P27/$2 


Using  M and  N the  specified  bit  of  PHI  CODE  Ifatrix  is  checked  to  find  out 
whether  the  representative  element  is  zero  or  non-zero. 


P28/1 


If  zero  then  transfers  to  P28/$l  where  M is  indexed  and  returns  to  the 
beginning  of  the  multiplying  loops  at  P27/$2. 


P28/2 ,3,4 

If  non-zero  Z(K,M)  multiplies  PHI(N,M)  and  sums  with  the  Nth  element  of 
TEMP  vector.  The  PHI  Pointer  is  then  incremented. 


P28/$l 


M is  incremented  by  one.  This  is  done  by  the  index  I which  is  incremented 
by  2 for  a P matrix,  18  for  and  1 for  X. 
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P28/$l+l 

If  M (or  I)  is  within  the  limits  then  there  are  more  elements  to  be 
multiplied  and  sequencing  returns  to  P27/$2. 

If  M exceeds  tit's  limit  then  N is  incremented,  M is  reinitialized. 


*28/1 

N is  tested  for  limit  value.  If  more  columns  of  PHI  to  process  then 
sequencing  branches  to  P27/$3.  If  not  then  the  K’th  row  operations  of 
Z on  modified  PHI  are  completed.  It  now  remains  to  add  in  those  elements 
which  multiply  the  identity  matrix. 

P29/$4  through  P30/1 

This  loop  adds  Z(K,K)  to  the  K'th  element  of  TEMP. 

P30/2.3 

TEMP  replaces  K'th  row  of  Z and  K is  indexed  by  1 . If  K exceeds  limit 
value  (1  or  9)  then  EXIT. 

If  not  then  sequencing  returns  to  P27/$5  for  another  Z row  operation, 
c)  Compute  BI  & C (pages  34-35) 

Generation  of  the  optimum  weighting  vector,  b.,  requires  three  expressions. 

1 T 

These  are*.  1)  the  predicted  measurement  variance  M^  P M 2)  measurement 

corruption,  or  confidence,  variance  C,,and  3)  the  divergence  control 

1 T 

factor  e.  lhe  matrix  P which  is  used  to  generate  M^PM  for  the  first 

measurement  residual  during  a measurement  update  is  that  which  resulted 
from  the  time-update  operation  on  X and  P. 

P34 /COMPUTE  BI  and  C through  P34 /COMPUTE  BI 
Qt  (K)  = Mt  P(K)  M^  + Ci 

bi ' [™?  * MIe]  [Q'‘] 
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The  B code  is  used  to  indicate  to  those  calculations  using  whether  the  first 

or  the  second  (inverse)  was  used.  It  is  desirable  to  maintain  b^  <1  so  that 
if  the  first  calculation  is  >1  then  the  second  is  used. 

P34/4.5. $6 

If  Q < 1/10  then  set  Q = 9/10 
If  Q > 1/10  then  set  Q = 9Q 

P35 

If  a position  measurement  then  Q = Q + 2 n mile 
If  a tracking  filter  measurement 
then  compute  upper  Kick  Limit, 

KQ„av  = (KICK  LIMIT)2  + .75  (cycle  )2 

MAX 

where  KICK  LIMIT  = V^,  the  phase  velocity  (constant) 

If  Q < KQ  set  Q = Q + .04  cycle 
Max 

“ Q > kqmx  s.t  Q - kqmx 
Q is  now  the  ACCEPT  LIMIT  or  KICK  LIMIT 

d)  P SYM  subroutine  (page  31) 

Forces  the  covariance  matrix  to  be  symmetric  and  the  diagonal  elements 
to  be  positive. 

If  P(i,i)  < 0 then  P (i,i)  set  to  0 

P(i,j)  = P(J,D  for  i ¥ j. 

ASSEMBLE  STATE  VECTOR  (page  32) 

Builds  a single  9-element  state  vector  (XX)  from  one  of  the  4 -element 
multiple  vectors  and  the  5-element  common  vector  (XXXX) . Refer  to 
description  of  P4/$6  for  further  description. 
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e)  AVERAGE  X (page  33) 


P33  computes  an  average  state  vector  and  average  variance  on  an  element 
by  element  basis. 

A A 


X 

avg 


X + 
avg 


X,  - X 


avg 


2 

a 

avg 


2 

a 

avg 


+ 


A 

X ) 

ave 


+ l 
N 


f)  Measurement  Update  X pages  (36-46),  also  Figure  2 (document  page  16). 

Introduction:  For  a position  fix  update  the  routine  is  entered  twice; 
once  with  the  North  position  error  and  once  with  the  East  position 
error.  There  are  three  arguments  as  inputs  at  each  entrance: 


1)  KICK/NO  KICK  Marker 

2)  POS  FIX/NO  POS  FIX  Marker 

3)  Position  Error  N or  E. 

Generation  of  New  State  Vectors:  Pages  36  through  42  are  concerned  with  the 
generation  of  new  state  vectors. 

Calculation  of  Residual,  P36 /MEASUREMENT  UPDATE  X through  P36/$98 


Res . . 
ik 


Y. 

l 


M. 

l 


Xk 


The  residual  combines  the  observation  with  each  available  state  vector  in 
turn.  The  residual  is  then  checked  to  see  if  it  is  reasonable. 

Verification  of  a valid  residual 

P36/$4 

The  residual  is  also  incremented  (and  decremented)  by  integer  (one  lane) 
values  and  tested  again  for  reasonableness.  New  state  vectors  are  computed 
if  the  criterion  is  passed.  This  incrementing  (decrementing)  continues 
until  the  reasonableness  test  is  failed.  Thus  several  new  state  vectors 
could  be  generated  from  a single  previous  state  vector.  It  is  also 
possible  that  X^  may  generate  no  new  state  vectors.  Once  the  system  has 

lane  resolved,  it  is  expected  that  only  one  state  vector  will  be  generated. 
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P36/$4+l 

This  test  checks  the  residual  against  KQ. 

ReSik  l<  KQi  | 

If  yes  then  sequencing  branches  to  P38/$l  where  a new  state  vector  is 
generated  and  validated.  If  NO  then  continue. 

P36/$4+2 

Is  the  KICK  Marker  and  the  POSITION  FIX  marker  true?  If  Yes  then,  again, 
branch  to  P38/$l  where  a new  state  vector  is  generated. 

If  no  then  continue. 

P37/1  through  P38/1 

This  page  describes  the  incrementation  and  decrementation  of  the  residual. 

This  is  done  only  with  tracking  filter  fixes.  For  a position  fix  only  the 
residual  itself  is  used. 

As  a result  of  this  loop  either  a new  state  vector  is  generated  or: 

If  it  is  a position  fix,  and  no  new  state  vectors  are  generated  and  there 
are  no  old  state  vectors  then  a misfire  is  generated  (see  description 
P44/$83);  if  there  are  old  state  vectors  then  sequencing  returns  to 
P36/$17. 

If  it  is  a tracking  filter  fix  then  several  conditions  must  be  met  to 
generate  a misfire: 

1)  There  are  no  old  state  vectors  to  process 

2)  There  are  no  new  state  vectors  generated 

2 

3)  The  accept  limit  is  not  less  than  (.15  cycle)  the  accept  limit  is 
the  value  of  Q from  the  compute  BI  & C subroutine 

4)  It  is  a good  tracking  filter  measurement,  and 

5)  It  is  not  the  base  station. 

I I 

1 
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P38/$l 


At  this  point  a residual  is  within  acceptable  limits,  i.e.,  within  the 
KQ  criterion  plus  or  minus  N lanes.  A new  state  vector  will  be  generated 
and  then  checked  for  acceptability  and  uniqueness  (against  other  new 
state  vectors). 


P38/$5 

(new)  = (old)  + 


if  B code  false 
if  B code  true 


b. 

i 

Res 


Res . . 
lk 


P38/$7  +1 

Did  the  1st  Position  Error  kick  the  system? 

If  so  then  it  is  known  that  the  position  fix  error  has  caused  an  out  of 
tolerance  condition.  Skip  the  36  n mile  test. 

If  not  then  continue. 


P38/$7+2 , 3 


Are  the  square  of  the  first  two  elements  of  the  state  vector  greater  than 
36  n miles!  If  so  then  the  vector  is  rejected  and  the  sequencing  returns 
to  the  residual  incrementation  or  decrementation  loop. 

If  not  then  continue. 


P39/$22+l  through  P40/2 

If  this  is  the  first  new  state  vector  generated  at  this  call  of  the 
Measurement  Update  X routine,  then  it  is  unique  and  branching  to  P41/$10 
skips  the  uniqueness  test.  If  the  current  vector  was  not  the  first  one 
generated  then  it  will  be  checked  against  all  currently  existing  new 
state  vectors  to  verify  uniqueness. 

If  it  is  not  unique,  i.e.,  if  the  difference  between  any  corresponding 
element  is  not  significant  then  the  vector  is  discarded  and  sequencing 
returns  to  the  residual  inc/dec  loop. 
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P40/ $13 

The  new  vector  has  been  found  unique  with  at  least  one  other  previously 
generated  new  state  vector.  It  must  be  checked  with  all  the  new  state 
vectors.  The  vector  pointer  is  incremented.  If  there  are  more  vectors 
then  the  vector  under  scrutiny  must  be  checked  against  the  others  for 
uniqueness.  Sequencing  returns  to  P39/$ll.  If  there  are  no  mo^e  new 
vectors  then  sequencing  continues. 


P40/$13+2 

If  the  vector  storage  area  is  full  then  declare  a misfire.  Otherwise 
continue  with  P41/$10. 


P41/$10  through  P41/$10+2 

The  state  vector  counter  is  incremented  and  the  vector  moved  to  a more 
permanent  location. 

The  average  state  vector  is  updated  and  sequencing  returns  to  the  residual 
inc/dec  loop  to  see  if  any  other  unique  vectors  can  be  generated  from  the 
original  "old'1  state  vector  that  generated  the  one  just  established. 


P41/$2 

This  is  the  sequence  described  in  P37/1.  This  point  is  reached  as  an 
exit  from  the  residual  inc/dec  loop.  All  possible  measurements  from  the 
old  state  vector  have  been  made  (one,  in  the  case  of  a position  fix). 


P4 l/$2+l , 2 

If  at  this  point  there  are  old  state  vectors  to  be  processed  then 
sequencing  returns  to  the  residual  inc/dec  loop. 

If  there  are  no  old  state  vectors  then  the  program  checks  to  see  if  there 
are  any  new  state  vectors. 

If  there  h.^ve  been  new  state  vectors  generated  (the  normal  case)  then 
sequencing  continues  to  P42/1  where  the  covariance  matrix  P,  is  updated. 
The  update  of  the  P matrix  will  be  described  in  the  MEASUREMENT,  UPDATE 
P subroutine. 

Now,  however,  if  there  are  no  new  state  vectors,  i.e.,  none  have  been 
generated,  then  a potential  misfire  situation  is  at  hand  and  must  be 
checked.  Sequencing  continues  on  P44/$83. 
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P42/$19 


If  this  is  a position  fix,  then  it  is  a misfire  situation.  The  position 
fix  has  indicated  a definite  location  for  the  system  to  within  the  specific 
lane.  If  the  system  position  disagrees  and  the  variances  on  both  positions 
are  small  enough  so  that  the  positions  are  mutually  exclusive,  then  a mis- 
fire situation  is  generated.  Sequencing  branches  to  P44/$83. 


P42/$19+l 

If,  however,  this  is  a tracking  filter  fix,  then  other  checks  must  be  made: 

2 

Is  Q < (.15  cycle) 

2 

If  so  then  set  Q = Q + (.15  cycle)  and  try  again  from  the  beginning; 
P36/MEAS.  UPDATE  X. 

If  not  then  continue  (on  P43/$24) 


P43/$24 , $28, $27 

2 2 
is  a ^ < (.00031  cycle  /sec) 

If  not  then  this  tracking  filter  fix  is  considered  to  be  unworthy  of 
further  consideration. 

Furthermore,  the  station  check  is  set  to  zero.  It  is  the  station  check 
which  determines  whether  the  system  is  confirmed.  There  are  24  indicators 
representing  the  "good  measurements"  from  each  station  and  frequency.  If 
three  stations  have  a history  of  good  measurements  on  all  three  frequencies 
since  there  was  only  one  state  vector,  then  the  system  is  confirmed. 


P43/$25  through  P43/$25+2 

If  the  tracking  filter  fix  is  good  and  it  is  not  in  the  phase  difference 
mode,  or  if  it  is  phase  difference  and  is  not  the  base  station  then  declare 
a misfire. 

If  the  TF  fix  is  good,  is  phase  difference,  and  is  the  base  station,  then 
this  measurement  is  ignored  and  processing  continues  to  the  next  frequency. 
If  no  more  frequencies  to  process,  then  exit  Kalman  without  an  update;  i.e. 
use  old  state  vectors. 
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Misfire : 

P44/$83 

2 

If  the  phase  rate  error  variance  ct  ^ does  not  exceed  (0.00031  cycle  / 

sec)^  the  failure  to  pass  the  reasonableness  criterion  means  that  either 
1)  the  present  estimate  of  position  is  incorrect,  or  2)  the  measurement 
is  incorrect  due  to  some  other  reason.  In  either  case  the  filter  is 
"opened  up"  by  setting  the  two  position  variances  to  the  larger  value 
between  their  present  value  and  the  value  at  sigma  squared  kick  (cr2kick). 
The  present  measurement  is  disregarded  and  the  filter  then  takes  the 
next  station  pair.  The  misfire  count  is  incremental  to  record  this 
occurence . 


g)  Measurement  Residue  (page  47) 

Computes  Y - MX  where  Y is  the  measurement,  M is  the  measurement 
matrix  and  X is  a state  vector. 

h)  MEASUREMENT  UPDATE  P (page  48) 

Covariance  Matrix  Generation 

If  any  new  state  vectors  are  generated  by  a measurement,  a measurement 
updated  covariance  matrix  is  computed: 

P(K)  = P(K)  - b.MiP(K)  + eM^bT 

If  no  new  state  vectors  are  generated  by  a measurement,  the  covariance 
matrix  is  not  measurement  updated. 

i)  ZERO  ROW- COLUMN  (page  49) 

Will  zero  any  row  and  column  of  a 9 x 9 matrix. 

j)  REGISTER  SET  UP  (Page  50) 

Creates  14  registers  and  sets  up  common  pointers. 


3.2  FLOW  DIAGRAMS 

The  Kalman  Filter  subprogram  flow  charts  are  presented  on  the  following 
pages . 
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Page  0001 


KALMAN  FILTER 

THIS  program  processes  TRACKING  FILTER  measurements  ako  position  fixes 
TO  GENERATE  ESTIMATES  OF  SYSTEM  ERRORS.  IT  IS  AN  OMEGA  TASK  THAT  IS 
ENTERED  5 MILLISECONOS  BEFORE  END  BURST.  THIS  PROGRAM  ONLY  CYCLES 
WHEN  THE  STATION  COUNTER  IS  0 (STATION  A).  IT  MAS  BEEN  SYNCHRONIZED 
WITH  THE  NON  OMEGA  TASK  ROUTINES  IN  SUCH  A way  THAT  RIJ  UPDATE  OCCURS 
5 MILLISECONDS  BEFORE  KALMAN.  FIVE  MILLISECONOS  AFTER  KALMAN  IS  INI- 
TIATED THE  END  BURST /TRACK | NO  FILTER  POUTINES  WILL  INTERRUPT  KALMAN  SO 
THAT  ALL  TRACKING  FILTERS  WILL  BE  TIMT  UPOATED  TO  THE  ENO  BURST  TlfC. 
THIS  RESULTS  IN  THE  RIJ.  KALMAN  ANO  TRACKING  FILTER  UPDATES  OCCURINO 
WITHIN  A 10  MILLISECOND  PER I 00.  FOR  THIS  TIMING  TO  BE  CORRECT  KALMAN 
MUST  NOT  READ  ANY  TRACKING  FILTER  DATA  IN  THE  5 MILLISECONOS  AVAILABLE 
BEFORE  THE  END  BURST  INTERRUPT. 
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Page  0002 


NO 


SET  NUMBER  OF  STATE  VECTORS  IN) 
TO  0 (FOR  1 STATE  VECTOR! 

RESET  NO  KICK  AND  STATION  CHECK 
ZERO  THE  FIRST  STATE  VECTOR 
ZERO  THE  COVARIANCE  MATRIX  (P) 

PRR  - .25  MICROSEC/SEC 
P33  • ( MH 1 . I 76 1 • *2  / 3 MICROSEC 
P22  - INITIAL  P22 
Pll  - INITIAL  Pll 
THE  INITIAL  Pll  AND  P22  ARE  T!C 
VALUES  FROM  KALMAN  A7  TURN  OFF 
IF  POSITION  ANO  TIhC  HAVE  NOT 
YET  BEEN  INSERTED.  THEY  ARE 
0.5  N MILE  IF  TirC  HAS  BEEN 
INSERTED  (NOT  OLD  TIME  ACCEPTED! 
ANO  THERE  IS  NO  POSITION  INSERT 
YET.  THEY  ARE  THE  INSERTED 
QUALITY  IF  POSITION  HAS  BEEN 
INSERTED 


1 

SET  SIGMA  KICK  - 

5 N Ml 

NO 


Save  - R] 
0003 


SI  1 

ooo: 
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1 

$11 

SET  KALMAN  STARTED  MARKER  MUCH 
RESETS  T*«  INITIALIZE  MARKER 

\ 

SAVE  RIJ 

SAVE  THE  RIJ  POSITION  MATRIX  NON 

TO  ESTABLISH  THE  CURRENT  OtCOA 
POSITION  FOR  KALMAN  COMPUTATIONS 

• COMPUTE  THE  0 FF  DIAGONAL  AM) 

• NON-UN  I TV  DIAGONAL  ELEKNTS 

• OF  THE  PHI  MATRIX  AM)  COMPUTE 

• ThC  R MATRIX 

• NOTE  - THERE  IS  SPACE  FOR  IN 

• ITEMS  IN  PHI  MATRIX 


' COMPUTE  PHI 

SET  FIRST  7 ITEMS  IN  PHI  TO  ZERO 

9TH  ITEM  - PHI  3.H 

3RO  ITEM  - PHI  1.8 

8TH  ITEM  - PHI  2.9 

I3TH  ITEM  - PHI  8.8 

IMTH  ITEM  - PHI  9.9 

R8  - R9  - -SIGMA(NAV) *BETA 

Rl  - R2  « R3  - RN  • 0 

R5.  R6,  R7  NOT  USED 


•TIME  UPDATE  COVARIANCE  MATRIX  (PI 
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• TlfC  UPOATE  STATE  VECTORS  «KI 
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RESCALE  XXXII)  ANO  XXXI?) 

TO  RAO  BO  FOR  OCLTA  THETA  2,  3 
SUM  1ST  2 TERMS  OF  SCATTER 
VECTOR  X2XX  TO  GET  ESTIMATE  OF 
POSITION  VARIATIONS  BCTtCCN 
STATE  VECTORS 


DOES  TFC  ^ 
POSITION  SCATTER  EXCEEO 
^ N.76  N MILES?  -- 


CORRECT  POSITION 
OCLTA  THETA  3 • - XXXI 1 1 
OCLTA  THETA  2 - XXX (2 I 

STORE  OCLTA  THETA ' S FOR  RIJ  UP- 
DATE TO  CORRECT  PRESENT  POSITION 


APPLY  CORRECTIONS  TO  POSITION 
KALMAN  SAVLO  AT  START  OF  CYCLE 
BSV  I ROTATE  RIJS 


OCLETE  CORRECTION  JUST  APPLIED 
TO  POSITION  FROM  ALL  STATE 
VECTORS 

X(N)  1 - X IN) 1 - XXXI 
X(N)2  - X(N)2  - XXX2 


ARE  there  more 
VECTORS  TO  CORRECT? 


DECAY  OCLTA  V2.  OCLTA  V3 
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• SELECT  MEASUREMENT  TTPC  - TAKE 

• POSITION  FI*  IF  AVAILABLE 

• OTHERWISE  TRY  FOR  A TRACK | NO 

• FILTER  MEASUREMENT 


J SELECT  MEASUREMENT 

SET  PROGRAM  flag  B FALSE  TO  NOTE 
THAT  SYSTEM  HAS  NOT  JUST  KICKEO 
OPEN  ON  A POSITION  FIXPOINT 
SET  M3, H, 5.6, 7, 8. 9 - 0 
(M  MATRIX  I 

SET  E3.H.5.6.7.8.9  - 0 
(DIVERGENCE  CONTROL) 


SET  PROGRAM  FLAG  A TRUE  FOR  A 
POSITION  FIX  A NO  FALSE  FOR  A 
TRACKING  FILTER  MEASURE fCNT 


IS  THIS  A 
POSITION  FIX? 


TRACKING  FILTER  MEAS 
URETCNT 

0012 


i POSITION  MEASUREMENT 

RESET  PROGRAM  FLAG  C fcHICH  IS 
USED  TO  COUNT  THE  2 MEASUREMENTS 
(NORTH  AND  EAST  ERRORS) 

SET  UP  SETS  OF  ARGUMENTS  FOR  TWO 
MEASUREMENTS 

FIRST  - ERROR  EAST,  COS  THETA  P. 

-SIN  THETA  P 

SECOND  - ERROR  NORTH,  SIN  ThCTA 
P/  COS  THETA  P 


±_*» 

SET  MEASUREMENT  - ERROR 
SET  Ml  - 2N0  ARGUMENT 
M2  - 3RO  ARGUMENT 
SET  MEASUREMENT  VARIANCE 

- INSERTED  POSITION  QUALITY 
SET  El  • E2  « POSITION  QUALITY 
CHANGE  STATE  OF  FLAG  C - GOES 
TRUE  IF  1ST  MEAS  OTHERWISE  FALSE 
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START  MEASUREMENT  PROCESS  1 NO 

] 

BSV 

3 

COMPUTE  Bl  AND  C 

COMPLETE  MEASUREMENT 

BSV 

1 MEASUREMENT  UPOATE 

• MEASUREMENT  UPDATE  X ROUTINE 

• RETURNS  HERE  ONLY  IF  A STATE 
■ VECTOR  HAS  UPOATEO. 

• IF  THE  MEASUREMENT  RESULTED 

• IN  NO  STATE  VECTORS  THEN  THE 

• RETURN  SKIPS  ONE  BOX  IN  THE 

• PROGRAM  FLOW. 
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V EXIT 
0029 
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PAGE 


• LOOK  for  MEASUREMENT  FROM  I OR 

• more  filters  for  any  station 


COMPUTE  Y - PI  I ♦ X 


0012 
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WAS  THf  PREVIOUS 

Measurement  from  the  base 
station? 


NO  MEASUREMENT  FROM  THIS  STATION 


PACE  0015 


CHAVE  ALL  STATIONS  BEEN  EXAMINED?: 


NO  MEASUREMENTS  THIS  TIME 


• THERE  IS  A MEASUREMENT  AVAILABLE 


SAVE  NUMBER  OF  THIS  STATION 


ARE  THERE  3 FREQ 
AVAILABLE  AND  IS  DIFF  FREQ 

marker  true? 


there  is  an  extra  freq  available 

READ  COUNTER  FOR  THIS  STATION 
THAT  DEFINES  OPTIMUM  FREQ  PAIR 
THE  COUNTER  CYCLES  BETWEEN  11—10 
11-13  ANO  13-10.  IT  IS  RESET  TO 
11-10  WHEN  NOT  IN  DIFF  FREQ 


SET  THE  N COUNTER  OF  THE  FREQ 
THAT  is  NOT  TC  BE  USED  TO  ZERO 
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it  too  far 

(120°) 


GET  POINTER  TO  SAVED  POSITION 
MATRIX 

> 

COMPUTE  AND  SAVE 
A NO  R . S3  FOR  M 

R . SI.  R . S2 
MATRIX 

BSV 

1 

MATRIX  30 

COMPUTE 

PREDICTED  CENTRAL  ANGLE 

BSV  I COMPUTE  THETA1 


SIN  THETA  1 


HAS  THE  BASE  ^ 
DATA  BEEN  COMPUTED  YET? 


JS  THE  STATION  TOO  CLOSE? 
(360  N MILE) 


SET  N COUNTER  AND  NUMBER  OF 
KALMAN  DUMPS  - 0 FOR  ALL  3 FREQ 


COMPUTE  POINTERS  FOR  STATION  AND 
BASE  PROPAGATION  DATA 


• START  MEASUREMENT  LOOP 
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PAGE  0030 


is  this  a 

pise  STATION  MEASUREMENT? 


READ  M>*  FROM  A TABLE 
NOTE  - Ml . M2  HILL  BE  ZERO 

USE  PHI  DOT  OR  FROM  TK.  TRACKING 

filter  as  the  predicted* 

MEASUREMENT  REFERENCE 


GET  STATION  NO..  FREQ  AND 
THETA  I ARGUMENTS  FOR  THETA  C 


COMPUTE  PREOICTED  TOTAL  PHASE 

FOR  THE  STATION . 

BSV  ' I THETA  C 


HASE  DIFFERENCE? 


GET  BASE  STATION  NO..  FREQ  AND 
THETA  I ARGUMENTS  FOR  THETA  C 


COMPUTE  PREOICTEO  TOTAL  PHASE 
FOR  THE  BASE  STATION 

BSV  THETA  C 

X 

FORM  PREDICTED  PHASE  DIFFERENCE 
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1 

, >51 

"(  OMPUTE  KALMAN  MEASUREKNT 
> TRACKING  FILTER  PMl  - 
PREOICTEO 

(OMPUTE  DIVERGENCE  CONTROL  El. 2 
••  K • MEAS  VAR  / (LAM80A)  ##2 


E3  - K • rCASVAR/IV  LAWAI”2 


SECONO  FREQ 
FORM  DIFFERENCES 


M - MI2TCI  - MC  1ST) , Ml  .2.3.4 


KICK  LIMIT  • (2ND)  - (1ST) 
MEAS  VAR  - (2ND)  ♦ CIST) 
MEASUREXNT  - C2N0)  - (1ST) 


s 

S90 

START  MEASUREMENT  PROCESSING 

] 

BSV 

5 

COMPUTE  Bl  AM)  C 

COMPLETE  MEASUREMENT 

BSV 

1 MEASUREMENT  UPOATE 

• MEASUREMENT  UPDATE  X ROUTINE 

• RETURNS  XRE  ONLY  IF  A STATE 

• VECTOR  MAS  UPDATED. 

• IF  NO  STATE  VECTORS  HERE  GEN- 

• ERATED  TFCN  IT  RETURNS  TO  SIO  P.0023 

• IF  ' GOOO  MEASUREMENT ' IS  FALSE 

• TO  TRY  ANOTHER  FREQ.  OTXRHISE 

• IT  RETURNS  TO  S92  AM5  EXITS 

• KALMAN 


$9 

00 
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PAGE  00?2 


V 

POST  bit  in  station  chcck  that 
IDENTIFIES  TK  STATION  AM)  FREO 
THAT  HAS  JUST  USED  FOR  A ME  AS 

this  IS  USED  AS  A CONFIRM  MARK 
WHEN  3 STA.  3 FREO  HAVE  MEAS'S 
IT  CONTROLS  WEN  SIGMA  KICK  IS 
REDUCED  TO  A MINIMUM  ANO  -FOR 
RHG-RHO-  WHEN  TO  OPEN  P33  ON 


A KICK 


RESET  POSTED  BIT  - ONLY  ALLOW 
CONFIRM  WHEN  NOT  AMBIGUOUS 


OPEN  ALL  TRACKING  FILTER 
VARIANCES 


S6S 


l 
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• RETURN  HERE  FROM  MEASUREMENT 


• UPOATE  IF  NO  STATE  VECTORS  WERE 

• generated  ano  the  system  was 

• NOT  KICKED  BECAUSE  THE  ‘6000 

• MEASUREMENT'  MARKER  WAS  FALSE 


SIO 

INCREMENT  POINTERS  TO  PREPARE 

FOR  THE  NEXT  FREQ 
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• KALMAN  CO*>LETC 

• LOOK  FOR  CONFIRM  A NO  EXECUTE 

• PROPAGATION  PREDICTION 


BBK 
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• INITIALIZE  PS.  P6.  P7 

• 

• THIS  ROUTINE  MILL  INITIALIZE  THE  STH.  6TH  AND  7TM  ROWS  AND  COLUMNS 

• Of  M P MATRIX 


B8K 
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PHI  (P>  PHI  TRANSPOSE 

THIS  ROUTINE  MULTIPLIES  PHI  BY  (X).  IP).  OR  (PITRANSPOSE.  THE  RESULT 
REPLACES  THE  ORIGINAL  MULTIPLIER.  THE  |H  PHI  ELEMENTS  CONSIST  OF 
0.  0.  PHKI.ei.  0.  0.  0.  0.  PHI  (2,9) , PHII3.M).  PHI  15, S) . PHII6.6), 
PHI (7.71.  PHI (8.8)  A NO  PHI (9.9) . THEY  ARE  ORDERED  BY  THE  SEQUENCE  OF 
USE  AGE  HHEN  ACCESSING  PHI  COLUMN  BY  COLUMN.  THE  UNITY  TERM  OF  T« 

01  AGONAL  ELEMENTS  DOES  NOT  APPEAR  AS  IT  IS  HANOLED  SEPARATELY. 


ENTER  HERE  HITH  J - 18  AND  I - 2 
TO  MULTIPLY  BY  P.  ENTER  HERE 
HITH  J - 2 ANO  I - 18  TO  MULTIPLY 
BY  P TRANSPOSE 


— — 1 

} 

PHI  P PHI T 

READ  POINTER  TO  P ANO  CREATE 

13  REGISTERS 

SET  UP  COUNTER  K-9  TO  PROCESS  P 

• ENTER  HERE  HITH  I - I.  POINTER  TO 

• X.  13  REGISTERS  ANO  COUNTER  K-l 

• TO  PROCESS  X (J  IS  NOT  USEOI 


1 


0027 


1 
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• THE  MULTIPLICATION  IS  COMPLETE 

• EXCEPT  FOR  THE  UNITY  TERM  IN 

• THE  PHI  DIAGONAL  ELEMENTS. 

• NOTE  - M.  N ARE  RESET  NOW 
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ADO  J TO  P MATRIX  POINTER  TO 
SEQUENCE  TO  THE  K*l  ROM  OR 
column  or  p (j-ie  roR  columns 

ANO  vl-2  roR  ROMS) 


EXIT 


BBK 


6' 
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• P SVM 

• 

• this  ROOT  INC  HILL  FORCE  THE  P MATRIX  TO  BE  STMCTRIC  AM)  THE  DIAGONAL 

• ELEMENTS  TO  BE  POSITIVE 


j P STM 


SET 

P(M.N) 

- P(N.M)  FOR 

M UNEQUAL  N 

SET 

PCM.N) 

- 0 IF  N - M 

ANO  P(M.N)  MINUS 

BBK 
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• ASSEMBLE  STATE  VECTOR 

• 

• THIS  ROUTINE  HILL  BUILD  A SINGLE  9 ELEMENT  STATE  VECTOR  IXX>  FROM  0»C 

• OF  THE  H ELEMENT  MULTIPLE  VECTORS  ANO  THE  5 ELEMENT  COMMON  VECTOR 

• IXXXX).  THE  ARGUMENT  IS  A POINTER  TO  THE  MULTIPLE  VECTOR. 


BBK 
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• AVERAGE  X 

• 

• THIS  ROUTINE  HILL  COMPUTE  AN  AVERAGE  STATE  VECTOR.  EACH  T IK  THIS 

• ROUTINE  IS  CALLED  IT  HILL  UPDATE  THE  AVERAGE  HITH  ONE  STATE  VECTOR. 

• IT  ALSO  GENERATES  THE  VARIANCE  OF  EACH  ELEMENT  OVER  TkC  SET  OF  STATE 

• VECTORS.  THE  ARGUKNTS  CONSIST  OT  POINTERS  TO  THE  MU  VECTOR  (xa>  AM) 

• THE  AVERAGE  VECTOR  <XC>  ANO  THE  NUMBER  OF  TfC  NEH  VECTOR  (N).  TK 

• RESULTS  ARC  LEFT  IN  XXX  (AVERAGE)  AM)  X2XX  (VARIANCE) 
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• COMPUTE  Bl  WC  C 

• THIS  ROUTItC  COMPUTES  TK  MEASUREMENT  HEIOMTINO  VECTOR  ■ 


• DC  TERM  I hC  KICK  CRITERIA 
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• MEASUREMENT  UPDATE  X 

• UPDATE  STATE  VECTORS  WITH  THIS  MEASUREMENT 


1 MEASUREMENT  UPOATE  X 

GENERATE  POINTERS  TO  THE  FIRST 
STATE  VECTOR  A NO  THE  FIRST  spare 
STORAGE  FOR  NEW  STATE  VECTORS 

SET  AVERAGE  STATE  VECTOR  CXXXI 

TO  ZERO  


SET  residual  INCREMENT  - ♦ I LANE 


GET  STATE  VECTOR  - N ELEMENTS 
FROM  MULTIPLE  ♦ S COMMON 

BSV  I ASSEMBLE  XX 


COMPUTE  MEASUREMENT  RESIDUAL 
BSV  I V MINUS  MX 


READ  THE  MEASUREMENT  RESIDUAL 


FORM  IRES  ♦/-  INC ) **2  FOR 
TESTING  AGAINST  KICK  LIMIT 


IS  THIS  RESIOUAL  ' 
INSIOE  THE  ACCEPTABLE  LIMIT? 


^ IS  this  a \ 
>1 T ION  FIX  THAT  HAS  ALREAOV 
— ■ — CAUSEO  A KICK? 


• THE  RESIDUAL  INCREMENT  WILL  BE  USED 
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• TO  TRY  TO  DRIVE  THE  RESIDUAL  INSIDE 

• THE  LIMITS.  the  INCREMENT  must  be 

• THE  OPPOSITE  SION  or  the  resioual. 

• THE  INCREMENT  is  ♦ FIRST  AND  TKN 

• MINUS  WHICH  IS  USED  AS  A MARKER  TO 

• DEFINE  PROCESS  IS  COMPLETE 
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EXAM  INC  NUMBER  OF  VECTORS 
COMPUTED  SO  EAR 


SET  UP  TO  TEST  VECTOR 
FOR  UNIQUENESS 


SI  I 

SET  TERM  COUNT  TO  ZERO 
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a 

REDUCE  TO  I VECTOR  K'.TH  JST  3 
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• the  new  state  vector  is  oooo 
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BBK 


• NO  NEW  STATE  VECTORS 
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IS  THIS  A GOOO  ^ 
tracking  filter  measurement? 


• DO  NOT  MISFIRE 


MODIFY  RETURN  TO  TRACKING  FILTER 
MEASUREMENT  TO  BYPASS  ANYMORE 
PROCESSING  OF  THIS  FREQ 


PUT  NEU  RETURN  AOORESS  IN  R1N 
SUBROUTINE  STACK 
SET  STATION  CHECK  TO  ZERO  TO 
FORCE  NEH  COUNT  BEFORE  CONFIRM 

RETURN 


IS  THIS  A POSITION  FIX? 


IS  THIS  PHASE  DIFFERENCE? 


IS  THIS  THE  BASE  STATION? 
(NO  MISFIRE) 
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EXAMINE  PZZ 


IS  SIGMA  KICK  LESS  THAN  PB2? 


SET  PSS  - SIGMA  KICK 


i S3N 

SAVE  NEW  P33 


IS  THIS  A POSITION  FIX? 


IS  KALMAN  IN 

HFFERENCE  FREQUENCY  NOW? 


RESET  FREQUENCY  PAIR  SELECT 
COUNTERS  FOR  ALL  STATIONS  TO 
ALWAYS  START  OIFF  FREQ  WITH 
11-10  OIFF  IF  3 FREQ  AVAILABLE 


MODIFY  RETURN  TO  TRACKING  FILTER 
MEASUREMENT  TO  BYPASS  ANYMORE 
MEASUREMENTS  FROM  THIS  STATION 
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• MEASUREMENT  RESIDUE 

• 

• THIS  ROUTINE  COMPUTES  THE  RESIDUE  7 - IMIIX).  TK  ARGUMENT  CONSISTS 

• OP  A POINTER  TO  X. 


1 

Y MINUS  MX 

COMPUTE  -(Ml (XI 
ADO  MEASUREMENT  ( Y1 
SAVE  RESIDUE 
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• MEASUREMENT  UFO  ATE  P 

* UPOATE  The  COVARIANCE  MATRIX  IE  NEW  STATE  VECTORS  WERE  GENERATED. 
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• REGISTER  SET  UP 

• THIS  ROUTINE  MILL  CREATE  I**  REGISTERS  AND  SET  UP  COMMON  POINTERS 


i REG  SETUP 

ATE  REGISTERS 
POINTER  TO  TEMP  VECTOR 
POINTER  TO  M MATRIX 
POINTER  TO  P MATRIX 
UP  TWO  9 COUNTERS 
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3.'.  COMPUTER  SUBPROGRAM  ENVIRONMENT 

3 . j *'  } man  Filter  Tables 

a)  Velocity  Variance:  This  table  is  used  to  define  the  initial  variance 
for  the  P matrix  diagonal  elements  for  velocity.  There  is  an  entry  in 
the  table  for  each  velocity  mode.  It  is  described  in  detail  in  the 
listing.  The  official  name  of  this  table  is  SIGMA  NRA. 

b)  Overflow  Protection:  This  table  is  used  to  define  the  maximum  value 
for  the  first  three  diagonal  elements  of  the  P matrix.  The  two  entries 
for  position  are  80  nautical  miles  squared  and  the  entry  for  T zero 

is  half  of  441  microseconds  squared.  The  table  is  described  in  the 
listing.  The  official  name  of  this  table  is  DATA  TABLE. 

c)  Tables  Proportional  to  Frequency:  There  are  four  tables  in  KALMAN 
that  have  three  entries  each  that  differ  only  in  frequency.  The 
first  entry  is  for  the  10.2  frequency,  the  second  is  for  13.6  and 
the  last  is  for  11.3.  The  official  names  of  these  tables  are 
LAMBDA  TABLE,  V LAMBDA,  M4  TABLE  and  TF  LAMBDA  CONSTANT  TABLE.  They 
are  described  in  detail  in  .the  listing. 

d)  Phi  Matrix  Definition:  This  table  identifies  the  elements  of  the  Phi 
matrix  that  are  not  zero  or,  in  the  case  of  the  diagonal  elements,  not 
unity.  The  first  nine  MSB's  of  each  entry  define  one  row  of  the  Phi 
Matrix.  The  nine  entries  define  the  nine  rows  of  Phi  in  order.  A one 
in  a bit  position  indicates  that  the  appropriate  term  is  a zero  (one 
unity).  The  official  name  of  this  table  is  PHI  CODE. 

e)  Station  Locations:  This  table  contains  the  location  of  all  existing 
OMEGA  transmitting  stations.  Each  location  is  specified  as  a three 
element  geocentric  position  A.  The  table  is  defined  in  detail  in  the 
listing.  The  official  name  of  this  table  is  STATION  VECTOR  TABLE. 


3.3.2  Temporary  Storage 

Kalman  requires  18  words  of  temporary  storage  called  KAL  TEMP  in  addition 
to  words  added  to  the  R15  pushdown  stack. 

3.3.3  Input /Output  Formats 
Not  applicable. 
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3.3.4  Required 

System  Library  Subroutines 

Subrout ine 

Flow  Diagram 

Subprogram  Design 
Document 

(by  Volume  Number) 

ATAN 

PI  7/4 

XII 

CROSS  PRODUCT 

PI  7/4 

XII 

DOT  PRODUCT 

PI  7/4 

XII 

D PHI  KI 

P20/4.7 

XII 

Matrix  30 

P17/2 

XII 

RESOLVE 

P6/2 

XII 

ROTATE  Reg's 

P5/4 

XII 

SIN 

P17/5 

XII 

SQRT 

PI  7/4 

XII 

THETA  1 

PI  7/4 

XII 

THETA  C 

P20/4 

P20/7 

XII 
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